#ifdef __WIN32__
    #include <windows.h>
#endif

#include "BM_camera.h"
#include <SDL/SDL.h>
#include <SDL/SDL_image.h>
#include <GL/gl.h>
#include <GL/glu.h>
#include <GL/glext.h>
#include <math.h>
#include <iostream>

/***********************************************************************************************************/
/* Team Black Voltage takes no credit for this file - credits: http://www.codecolony.de/opengl.htm#camera2 */
/***********************************************************************************************************/

SF3dVector F3dVector ( GLfloat x, GLfloat y, GLfloat z )
{
	SF3dVector tmp;
	tmp.x = x;
	tmp.y = y;
	tmp.z = z;
	return tmp;
}
SF3dVector AddF3dVectors (SF3dVector* u, SF3dVector* v)
{
	SF3dVector result;
	result.x = u->x + v->x;
	result.y = u->y + v->y;
	result.z = u->z + v->z;
	return result;
}
void AddF3dVectorToVector ( SF3dVector * Dst, SF3dVector * V2)
{
	Dst->x += V2->x;
	Dst->y += V2->y;
	Dst->z += V2->z;
}


/***************************************************************************************/

BMCamera::BMCamera()
{
	//Init with standard OGL values:
	Position = F3dVector (	0.0,
							0.0,
							0.0);
	ViewDir = F3dVector(	0.0,
							0.0,
							-1.0);
	ViewDirChanged = false;
	//Only to be sure:
	RotatedX = RotatedY = RotatedZ = 0.0;
	mouse_move = 0;
	lastx = lasty = 0;
}

void BMCamera::reset()
{
	//Init with standard OGL values:
	Position = F3dVector (	0.0,
							0.0,
							0.0);
	ViewDir = F3dVector(	0.0,
							0.0,
							-1.0);
	ViewDirChanged = false;
	//Only to be sure:
	RotatedX = RotatedY = RotatedZ = 0.0;
	mouse_move = 0;
	lastx = lasty = 0;
}

void BMCamera::mouseMove(float x, float y, int windowWidth, int windowHeight, int use_joystick, float sensitivity)
{

	//static int mouse_move = 0;
    /*float diffx=(x-lastx) / 2; //check the difference between the current x and the last x position
    float diffy=(y-lasty) / 2; //check the difference between the current y and the last y position
    lastx=x; //set lastx to the current x position
    lasty=y; //set lasty to the current y position
    RotatedX -= diffy; //set the xrot to xrot with the addition of the difference in the y position
    RotatedY -= diffx;// set the xrot to yrot with the addition of the difference in the x position
    */
	int dif = 50;
    if (use_joystick){ // in case we need to do something different when usign the joystick
        if( (lastx - x) >dif || (lastx - x) <-dif ||
            (lasty - y) >dif || (lasty - y) <-dif )
        {
            lastx = x;
            lasty = y;
        }

        RotatedY += (((GLfloat)lastx - (GLfloat)x)/2);
        lastx = x;

        if(RotatedY>360)
            RotatedY-=360;
        if(RotatedY<0)
            RotatedY+=360;

        RotatedX += (((GLfloat)lasty - (GLfloat)y))/2;
        lasty = y;

        if(RotatedX > 90)
            RotatedX = 90.0;
        if(RotatedX<-90)
            RotatedX = -90.0;
    }
    else {

        if( (lastx - x) >dif || (lastx - x) <-dif ||
            (lasty - y) >dif || (lasty - y) <-dif )
        {
            lastx = x;
            lasty = y;
        }

        RotatedY += (((GLfloat)lastx - (GLfloat)x)/2);
        lastx = x;

        if(RotatedY>360)
            RotatedY-=360;
        if(RotatedY<0)
            RotatedY+=360;

        RotatedX += (((GLfloat)lasty - (GLfloat)y))/2;
        lasty = y;

        movedY = movedY - RotatedX;

        if(RotatedX > 90)
            RotatedX = 90.0;
        if(RotatedX<-90)
            RotatedX = -90.0;
		if(mouse_move % 3 == 0 ){
			int middleX = (int)windowWidth  >> 1;   // This is a binary shift to get half the width
			int middleY = (int)windowHeight >> 1;	// This is a binary shift to get half the height

			float angleY = (float)( (middleX - x) ) / (1.0f * sensitivity);
			float angleX = (float)( (middleY - y) ) / (1.0f * sensitivity);

			RotatedY += angleY;
			RotatedX += angleX;

			SDL_WarpMouse(middleX, middleY);
        }
    }
    ViewDirChanged = true;

    /*
    	POINT mousePos;									// This is a window structure that holds an X and Y
	int middleX = SCREEN_WIDTH  >> 1;				// This is a binary shift to get half the width
	int middleY = SCREEN_HEIGHT >> 1;				// This is a binary shift to get half the height
	float angleY = 0.0f;							// This is the direction for looking up or down
	float angleZ = 0.0f;							// This will be the value we need to rotate around the Y axis (Left and Right)
	static float currentRotX = 0.0f;

	// Get the mouse's current X,Y position
	GetCursorPos(&mousePos);

	// If our cursor is still in the middle, we never moved... so don't update the screen
	if( (mousePos.x == middleX) && (mousePos.y == middleY) ) return;

	// Set the mouse position to the middle of our window
	SetCursorPos(middleX, middleY);

	// Get the direction the mouse moved in, but bring the number down to a reasonable amount
	angleY = (float)( (middleX - mousePos.x) ) / 500.0f;
	angleZ = (float)( (middleY - mousePos.y) ) / 500.0f;

	// Here we keep track of the current rotation (for up and down) so that
	// we can restrict the camera from doing a full 360 loop.
	currentRotX -= angleZ;

	// If the current rotation (in radians) is greater than 1.0, we want to cap it.
	if(currentRotX > 1.0f)
		currentRotX = 1.0f;
	// Check if the rotation is below -1.0, if so we want to make sure it doesn't continue
	else if(currentRotX < -1.0f)
		currentRotX = -1.0f;
	// Otherwise, we can rotate the view around our position
	else
	{
		// To find the axis we need to rotate around for up and down
		// movements, we need to get a perpendicular vector from the
		// camera's view vector and up vector.  This will be the axis.
		CVector3 vAxis = Cross(m_vView - m_vPosition, m_vUpVector);
		vAxis = Normalize(vAxis);

		// Rotate around our perpendicular axis and along the y-axis
		RotateView(angleZ, vAxis.x, vAxis.y, vAxis.z);
		RotateView(angleY, 0, 1, 0);
	}
	*/
}

void BMCamera::GetViewDir( void )
{
	SF3dVector Step1, Step2;
	//Rotate around Y-axis:
	Step1.x = cos( (RotatedY + 90.0) * PIdiv180);
	Step1.z = -sin( (RotatedY + 90.0) * PIdiv180);
	//Rotate around X-axis:
	double cosX = cos (RotatedX * PIdiv180);
	Step2.x = Step1.x * cosX;
	Step2.z = Step1.z * cosX;
	Step2.y = sin(RotatedX * PIdiv180);
	//Rotation around Z-axis not yet implemented, so:
	ViewDir = Step2;
}
void BMCamera::MoveTo(SF3dVector Direction){
	AddF3dVectors(&Position, &Direction );
}

void BMCamera::Move (SF3dVector Direction){
	AddF3dVectorToVector(&Position, &Direction );
}

void BMCamera::RotateY (GLfloat Angle){
	RotatedY += Angle;
	ViewDirChanged = true;
}

void BMCamera::RotateX (GLfloat Angle)
{
	RotatedX += Angle;
	ViewDirChanged = true;
}

void BMCamera::Render( void )
{
	glRotatef(-RotatedX , 1.0, 0.0, 0.0);
	glRotatef(-RotatedY , 0.0, 1.0, 0.0);
	glRotatef(-RotatedZ , 0.0, 0.0, 1.0);
	glTranslatef( -Position.x, -Position.y, -Position.z );
}

void BMCamera::MoveForwards( GLfloat Distance )
{
	if (ViewDirChanged) GetViewDir();
	SF3dVector MoveVector;
	MoveVector.x = ViewDir.x * -Distance;
	MoveVector.y = ViewDir.y * -Distance;
	MoveVector.z = ViewDir.z * -Distance;
	AddF3dVectorToVector(&Position, &MoveVector );
}

void BMCamera::StrafeRight ( GLfloat Distance )
{
	if (ViewDirChanged) GetViewDir();
	SF3dVector MoveVector;
	MoveVector.z = -ViewDir.x * -Distance;
	MoveVector.y = 0.0;
	MoveVector.x = ViewDir.z * -Distance;
	AddF3dVectorToVector(&Position, &MoveVector );
}

GLfloat BMCamera::getX(){
	return Position.x;
}
GLfloat BMCamera::getY(){
	return Position.y;
}
GLfloat BMCamera::getZ(){
	return Position.z;
}
GLfloat BMCamera::getAngleX(){
	return RotatedX;
}
GLfloat BMCamera::getAngleY(){
	return RotatedY;
}
GLfloat BMCamera::getAngleZ(){
	return RotatedZ;
}
GLfloat BMCamera::getMovedY(){
	return movedY;
}

void BMCamera::setX(float sx){
	Position.x = sx;
}
void BMCamera::setY(float sy){
	Position.y = sy;
}
void BMCamera::setZ(float sz){
	Position.z = sz;
}

void BMCamera::setAngleX(float sx){
	RotatedX = sx;
}
void BMCamera::setAngleY(float sy){
	RotatedY = sy;
}
void BMCamera::setAngleZ(float sz){
	RotatedZ = sz;
}

void BMCamera::setPosition(SF3dVector vector){
	setX(vector.x);
	setY(vector.y);
	setZ(vector.z);
}

